module I2CDispatcherP {
	provides {
		interface StdControl;
	}
	uses {
		interface AsyncStdControl as I2CControl;
		interface Atm128I2CSlave as I2CSlave;
		
		interface Leds;

//		interface MotorControl as TowerMotor;
//		interface Timer<TMilli> as TowerTimer;
		interface Timer<TMilli> as SensorTimer;
		interface Timer<TMilli> as LightTimer;

//		interface Timer<TMilli> as LedTimer;
		
		interface DoForSomeTime as BarrelUpDown;
		interface DoForSomeTime as Shoot;

		interface Read<uint16_t> as Sensor;
		interface Set<uint8_t> as Output;
		
		interface TowerRotate;

		interface GeneralIO as TopLed;
		interface Timer<TMilli> as TopLedTimer;
	}
}

implementation {
	enum {
		CMD_NONE = 0,
		CMD_TURN_ST = 1,      // speed (-127..127) and time (0..255 * 0.1s)
		CMD_TURN_POS = 2,     // position (int16_t)
		CMD_TURN_RELPOS = 3,  // position (int16_t)
		CMD_UPDOWN_T = 4,     // time (0..255 * 0.1s)
		CMD_UPDOWN_PARK = 5,
		CMD_FIRE_T = 6,         // time (0..255 * 0.1s)
		CMD_WRITE = 7,        // 8bit output
		CMD_CALIBRATE = 8,
		CMD_SEARCH_START = 9,
		CMD_SEARCH_STOP = 10,
		IDLE,
		READ,
		ERROR
	};
	uint8_t state = IDLE;
	uint8_t index = 0, remain = 0;
	uint8_t puffer[2];

	uint8_t readidx = 0;
	
	bool searching = FALSE;
	uint16_t maxvalue = 0; 
	 
	command error_t StdControl.start() {
		call I2CSlave.setSlaveAddress(42, FALSE);
		
		call SensorTimer.startPeriodic(50);
//		call Leds.set(0xAA);
		return call I2CControl.start();
	}
	
	command error_t StdControl.stop() {
		return call I2CControl.stop();
	}

	async event void I2CSlave.startWrite() {
		state = IDLE;
//		call Leds.set(0x1);
	}

	// Master -> slave
	async event bool I2CSlave.write(uint8_t data) {
//		call Leds.set(0x3);
		call Leds.set(call Leds.get()|(1<<1));
		if (state == IDLE) {
			if (data < IDLE) {
				state = data;
				index = 0;
				switch (state) {
					case CMD_NONE:
						remain = 0;
						break;
					case CMD_TURN_ST:
					case CMD_TURN_POS:
					case CMD_TURN_RELPOS:
						remain = 2;
						break;
					case CMD_UPDOWN_T:
						remain = 1;
						break;
					case CMD_UPDOWN_PARK:
						remain = 0;
						break;
					case CMD_FIRE_T:
					case CMD_WRITE:
						remain = 1;
						break;
					case CMD_CALIBRATE:
					case CMD_SEARCH_START:
					case CMD_SEARCH_STOP:
						remain = 0;
						break;
					default:
						remain = 0;
						state = ERROR;
						break;
				}
//				call Leds.set(data);
			} else
				state = ERROR;
		} else if (state < IDLE) {
			puffer[index++] = data;
			remain--;
		}
		if (state != ERROR && remain == 0) {
			switch (state) {
				case CMD_NONE:
					break;
				case CMD_TURN_ST:
//					call TowerMotor.setSpeed(((int16_t)(puffer[0]))<<8);
//					call TowerTimer.startOneShot( ((uint16_t)(puffer[1]))*102 );
					{
						uint16_t time = ((uint16_t)(puffer[1]))*102;
						call TowerRotate.turnST(puffer[0], time);
						call TopLed.set();
						call TopLedTimer.startOneShot(time + 1000);
					}
					break;
				case CMD_TURN_POS:
					call TowerRotate.turnPos(puffer[0]+(((uint16_t)puffer[1])<<8));
					call TopLed.set();
					call TopLedTimer.startOneShot(5000);
					break;
				case CMD_TURN_RELPOS:
					call TowerRotate.turnRelPos(puffer[0]+(((uint16_t)puffer[1])<<8));
					call TopLed.set();
					call TopLedTimer.startOneShot(5000);
					break;
				case CMD_UPDOWN_T:
					call BarrelUpDown.doit(((uint16_t)(puffer[0]))*102);
					break;
				case CMD_UPDOWN_PARK:
					break;
				case CMD_FIRE_T:
					call Shoot.doit(((uint16_t)(puffer[0]))*102);
					break;
				case CMD_WRITE:
					call Output.set(puffer[0]);
					break;
				case CMD_CALIBRATE:
					break;
				case CMD_SEARCH_START:

					
					call TowerRotate.turnST(32, 10000);
					call LightTimer.startOneShot( 10000);

					call TopLed.set();
					call TopLedTimer.startOneShot(10000 + 1000);
//					call SensorTimer.startPeriodic(50);
					maxvalue = 0;
					searching = TRUE;
					break;
				case CMD_SEARCH_STOP:
					break;
			}
			state = IDLE;
		}
		return TRUE;		
	}

	// End of master -> slave transmission
	async event void I2CSlave.stopWrite() {
//		call Leds.set(call Leds.get()|(1<<3));
		state = IDLE;
	}


	async event void I2CSlave.startRead() {
		state = READ;
	}

	// Slave -> master
	async event error_t I2CSlave.readRequest() {
//		call I2CSlave.read(20);
//		return SUCCESS;
		return call Sensor.read();

/*		switch (readidx++) {
			case 0:
				return call Sensor.read();
				break;
			case 1: ;
			case 2: ;
			case 3: ;
			case 4: ;
		}
		return SUCCESS;
*/
	}


	event void Sensor.readDone(error_t result, uint16_t val ) {
		call Leds.set(val>>8);
		if (searching) {
			if (val > maxvalue)
				maxvalue = val;
			if (val < maxvalue/2 ) {

				call TowerRotate.turnST(-64, 170);

//				call SensorTimer.stop();
				searching = FALSE;
				call Output.set(7);
				call LightTimer.startOneShot(6000);
			}
		} else if (state == READ) {
			call I2CSlave.read(val);
		}
	}
	
	// End of slave -> master transmission
	async event void I2CSlave.stopRead() {
		readidx = 0;
	}
	
//	event void TowerTimer.fired() {
//		call TowerMotor.setSpeed(0);
//	}

	event void SensorTimer.fired() {
		call Sensor.read();
	}

	event void LightTimer.fired() {
		if (searching)
			searching = FALSE;
		else
			call Output.set(0);
	}

	event void TopLedTimer.fired() {
		call TopLed.clr();
	}

}
